﻿using System;
using System.Collections.Generic;
using System.Diagnostics.Contracts;
using System.Linq;
using System.Text;
using EuroManager.Domain.Simulator.Helpers;

namespace EuroManager.Domain.Simulator.Engine.Movements
{
    public class ArriveMovement : IMovement
    {
        private bool isFinished = false;
        private Vector2D location;
        private Vector2D velocity;
        private Vector2D destination;
        private float acceleration;
        private float maxSpeed;
        private float deceleration;
        private float tolerance;
        private float desiredTime;

        public ArriveMovement(
            Vector2D location, Vector2D velocity, Vector2D destination, float acceleration,
            float deceleration, float maxSpeed, float tolerance, float desiredTime)
        {
            Contract.Requires(acceleration.IsRational() && acceleration > 0);
            Contract.Requires(deceleration.IsRational() && deceleration > 0);
            Contract.Requires(maxSpeed.IsRational() && maxSpeed > 0);
            Contract.Requires(tolerance.IsRational() && tolerance >= 0);
            Contract.Requires(desiredTime.IsRational() && desiredTime >= 0);

            this.location = location;
            this.velocity = velocity;
            this.destination = destination;
            this.acceleration = acceleration;
            this.maxSpeed = maxSpeed;
            this.deceleration = deceleration;
            this.tolerance = tolerance;
            this.desiredTime = desiredTime;
        }

        public bool IsFinished
        {
            get { return isFinished; }
        }

        public Vector2D Location
        {
            get { return location; }
        }

        public Vector2D Velocity
        {
            get { return velocity; }
        }

        public void Update(float elapsedTime)
        {
            float distance = (destination - location).Length;

            if (distance < tolerance)
            {
                // Reached the target.
                isFinished = true;
            }
            else
            {
                Vector2D accelerationVector = CalculateAcceleration();

                velocity = Physics.Velocity(velocity, accelerationVector, elapsedTime, maxSpeed);
                location = Physics.Location(location, velocity, elapsedTime);
            }
        }

        private Vector2D CalculateAcceleration()
        {
            Vector2D currentDirection = velocity.Normalized();
            Vector2D desiredDirection = (destination - location).Normalized();

            float speed = velocity.Length;
            float turningSpeed = CalculateTurningSpeed(currentDirection, desiredDirection);

            if (speed > turningSpeed)
            {
                // Decelerate before turning.
                return -currentDirection * deceleration;
            }
            else
            {
                // Turn - change the direction.
                velocity = desiredDirection * speed;

                return CalculateAccelerationAfterTurn();
            }
        }

        private float CalculateTurningSpeed(Vector2D currentDirection, Vector2D desiredDirection)
        {
            float cos = Vector2D.Dot(currentDirection, desiredDirection);
            float turnFactor = (1 + cos) / 2;

            return maxSpeed * turnFactor;
        }

        private Vector2D CalculateAccelerationAfterTurn()
        {
            Vector2D toTarget = destination - location;
            float speed = velocity.Length;
            float distance = toTarget.Length;
            
            float stopDistance = Physics.Distance(speed, deceleration);

            if (distance <= stopDistance)
            {
                // Start decelerating.
                return -toTarget.Normalized() * deceleration;
            }
            else
            {
                // Can still accelerate.
                if (speed > 0)
                {
                    float stopTime = Physics.Time(speed, 0, -deceleration);
                    float timeWithCurrentSpeed = Physics.Time(speed, distance - stopDistance);

                    if (timeWithCurrentSpeed + stopTime < desiredTime)
                    {
                        // Do not increase the speed - will reach destination faster than required.
                        return Vector2D.Zero;
                    }
                }

                return toTarget.Normalized() * acceleration;
            }
        }
    }
}
